#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2
import math
import serial
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped
from std_msgs.msg import String
#from scipy.spatial.transform import Rotation as R

real_distance=-1
my_time=-1.0
integral=0
def bomber(velocity_ahead,velocity_height,target_x ,height ,distance):
    global real_distance
    global my_time
    global integral
    #global velocity_ahead
    #global target_x 
    #global height 
    #global real_z 
    g=9.8
    advance=1
    error_range=0.4
    loss_real_distance=-1
    print("velocity_ahead:",velocity_ahead)
    print("velocity_height:",velocity_height)
    print("height:",height)
    #print("real_z:",real_z)
    #print("target_x:",target_x)
    if(velocity_ahead>-1 and height>=0 and distance>=0):
        
        t=(-velocity_height+math.sqrt(velocity_height**2-2*g*(-height)))/(2*0.5*g)
        bom_distance=velocity_ahead*t+advance
        print("bom_distance:",bom_distance)
        if(distance==real_distance):
            integral=integral+(rospy.get_time()-my_time)*velocity_ahead*2
            my_time=rospy.get_time()
            loss_real_distance=real_distance-integral
            print("loss_real_distance",loss_real_distance)
        else:    
            real_distance=distance
            integral=0
            my_time=rospy.get_time()
        print("real_distance:",real_distance)
        if((bom_distance-error_range<=real_distance and real_distance<=bom_distance+error_range) or (bom_distance-error_range<=loss_real_distance and loss_real_distance<=bom_distance+error_range)):
            try:
                portx="/dev/ttyTHS1"
                bps=9600
                timex=5
                ser=serial.Serial(portx,bps,timeout=timex)
                result=ser.write("open\r\n".encode("gbk"))
                #print("%d",result)
                ok= "bomber_finish"
                #打印消息到屏幕上；把消息写入节点的日志文件中；写入rosout。
                #rosout是一个方便的调试工具：您可以使用rqt_console来拉取消息，而不必在控制台窗口找你节点的输出
                rospy.loginfo(ok)
                massage_pub.publish(ok)
                ser.close()
                

            except Exception as e:
                print("bomber_error",e)


class image_converter:
    def __init__(self):    
        self.velocity_ahead=-1
        self.velocity_height=0
        self.target_x      =-1
        self.target_y      =-1
        self.height        =-1
        self.real_z        =-1
        self.p             =361
        self.distance      =-1
        self.height_sensor =-1
        # 创建cv_bridge，声明图像的发布者和订阅者
        self.bridge = CvBridge()
        self.depth_image_sub = rospy.Subscriber("/camera/aligned_depth_to_color/image_raw", Image, self.callback)
        rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.UAVpositionCallback)
        rospy.Subscriber("/mavros/local_position/velocity_body", TwistStamped, self.UAVvelocityCallback)
        rospy.Subscriber("talker", PoseStamped, self.targetCallback)
        rospy.Subscriber("bomber/start", String, self.bomberCallback)
    def UAVvelocityCallback(self,data):
        #global velocity_ahead
        try:
            vx=data.twist.linear.x
            vy=data.twist.linear.y
            vz=data.twist.linear.z
        except:
            print("velocity_ahead error")
        #print("vx",vx)
        #print("vy",vy)
        #print("vz",vz)
        self.velocity_ahead=vx
        self.velocity_height=-vz
        #print(velocity_ahead)


    def targetCallback(self,data):
        
        #global target_x     
        try:
            self.target_x=data.pose.position.x
            self.target_y=data.pose.position.y
        except:
            print("target error")
        
        
        
    def UAVpositionCallback(self,data):   
        #global height          
        try:
            self.height_sensor=data.pose.position.z
            #MyEuler = R.from_quat([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]).as_euler('zyx')
            x = data.pose.orientation.x
            y = data.pose.orientation.y
            z = data.pose.orientation.z
            w = data.pose.orientation.w
            #r = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
            #r = r / math.pi * 180
            p = math.asin(2 * (w * y - z * x))
            self.p = p / math.pi * 180
            #y = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
            #y = y / math.pi * 180
            #print("\n源坐标系到目标坐标系旋转顺序为X,Y,Z,左手系．")
            #print("绕x轴旋转角度: {}".format(r))#roll
            #print("绕y轴旋转角度: {}".format(self.p))#pitch
            #print("绕z轴旋转角度: {}\n".format(y))#yew
        except:
            print("height error")
        #print(height)
    def callback(self,data):
        #global real_z    
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        #massage_pub.publish("start")        
        try:
            depth_img = self.bridge.imgmsg_to_cv2(data, "16UC1")
            depth_img = cv2.resize(depth_img, (640, 480))
            #print(depth_img.shape)
            #cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            #cv_image = cv2.resize(cv_image, (1280, 720))
        except CvBridgeError as e:
            print (e)
        x = int(self.target_x)
        y = int(self.target_y)
        fx = 605.4
        fy = 605.5
        ppx = 327.6
        ppy = 230.2
        #print(x,y)
        if(x>=0 and y>=0):
            depth=depth_img[y, x]
            self.real_z = depth * 0.001 *math.cos((y-ppy)/480*58/180 * math.pi)  # mm转换m
            self.real_x = (x - ppx) / fx * self.real_z
            self.real_y = (y - ppy) / fy * self.real_z
            print("depth",depth)
            print("self.real_x",self.real_x)
            print("self.real_y",self.real_y)
            print("self.real_z",self.real_z)
            if(self.p!=361):
                pitch=(self.p+40)/180 * math.pi
                ty=-0.010
                tz=0.0145
                #pitch=math.sin(90)
                #print("target_pitch:",pitch)
                Xc=self.real_x
                Yc=math.cos(pitch)*self.real_y+math.sin(pitch)*self.real_z  +ty
                Zc=-math.sin(pitch)*self.real_y+math.cos(pitch)*self.real_z +tz
                print("Yc_height",Yc)
                print("Zc_height",Zc)
                if(Yc<=8):
                    self.height=Yc
                    self.distance=Zc
                #elif(self.height_sensor>0):
                #    print("height error!")
                #    self.height=self.height_sensor
                #    self.distance=self.height_sensor/Yc*Zc
                else:
                    print("height and height_sensor error!")
                #print("self.distance",self.distance)
                #print("self.height",self.height)
        #print(real_z)
    def bomberCallback(self,data):
        #print(data)
        if(data.data=="start"):
            #print(1)
            bomber(self.velocity_ahead,self.velocity_height,self.target_x ,self.height ,self.distance)


if __name__ == '__main__':
   
    #try:
        # 初始化ros节点
    rospy.init_node("bomber_test")
    rospy.loginfo("Starting bomber_test node")
    massage_pub = rospy.Publisher("bomber/finish",String, queue_size=1)
    image_converter()
    rospy.spin()
    
    #except KeyboardInterrupt:
        #print ("Shutting down cv_bridge_test node.")
        #cv2.destroyAllWindows()
